#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <opencv2/opencv.hpp>

bool scan_received = false;

struct ModelPoint
{
    ModelPoint(int x,int y,double residual):x_(x),y_(y),residual_(residual)
    {

    }

    int x_;
    int y_;
    double residual_;
};

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    // if(scan_received)
    //     return;
    //1.将激光点数据转换成img数据
    cv::Mat img(500,500,CV_8UC1,cv::Scalar(0));
    cv::Point2d center(250,250);
    double resolution = 0.05;
    std::cout<<"size:"<<msg->ranges.size()<<std::endl;
    size_t count = 0;
    for(size_t i =0;i<msg->ranges.size();i++)
    {
        if(msg->ranges[i]>18 || msg->ranges[i]<0.1)
            continue;
        double x = (msg->ranges[i]*cos(msg->angle_min+i*msg->angle_increment));
        double y = (msg->ranges[i]*sin(msg->angle_min+i*msg->angle_increment));
        
        
        int xx = static_cast<int>(x/resolution) + center.x;
        int yy = center.y - static_cast<int>(y/resolution);
        if(xx>=0 && xx<img.cols && yy>=0 && yy<img.rows)
        {
            img.at<uchar>(yy, xx) = 255;
            count++;
        }
    }
    std::cout<<"count:"<<count<<std::endl;

    //设置似然场地

    scan_received = true;
    cv::imshow("map", img);
    cv::waitKey(1);

}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "likelihood");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/f_scan", 1, scanCallback);
    ros::spin();
    return 0;

}